#include <errno.h>
#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

#include "ecrt.h"
#include "./tinyxml2.h"
 
#define false 0
#define true 1
#define RUNTIME_MS 60000 // 60s
#define PERIOD_NS 1000000 // 1ms
#define PERIOD_MS (PERIOD_NS / 1000000)
#define ETHERCAT_STATUS_OP 0x08
#define STATUS_SERVO_ENABLE_BIT (0x04)
 
// master status
typedef enum _SysWorkingStatus
{
    SYS_WORKING_POWER_ON,
    SYS_WORKING_SAFE_MODE,
    SYS_WORKING_OP_MODE,
    SYS_WORKING_LINK_DOWN,
    SYS_WORKING_IDLE_STATUS // 系统空闲
} SysWorkingStatus;
typedef struct _GSysRunningParm
{
    SysWorkingStatus m_gWorkStatus;
} GSysRunningParm;
GSysRunningParm gSysRunning;
 
pthread_t InterpolationTask;
int run = 1;
int ecstate = 0;
 
#define CLOCK_TO_USE CLOCK_REALTIME
#define NSEC_PER_SEC (1000000000L)
#define TIMESPEC2NS(T) ((uint64_t)(T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
 
static int64_t system_time_base = 0LL;
// 获取当前系统时间
uint64_t system_time_ns(void)
{
    struct timespec rt_time;
    clock_gettime(CLOCK_TO_USE, &rt_time);
    uint64_t time = TIMESPEC2NS(rt_time);
    return time - system_time_base;
}
 
/****************************************************************************/
// EtherCAT
ec_master_t *master = NULL;
static ec_master_state_t master_state = {};
static ec_domain_t *domainServoInput = NULL;
static ec_domain_state_t domainServoInput_state = {};
static ec_domain_t *domainServoOutput = NULL;
static ec_domain_state_t domainServoOutput_state = {};
static uint8_t *domainOutput_pd = NULL;
static uint8_t *domainInput_pd = NULL;

// 两套从站配置
static ec_slave_config_t *sc_x_axis = NULL;  // X轴配置
static ec_slave_config_t *sc_y_axis = NULL;  // Y轴配置
static ec_slave_config_state_t sc_x_axis_state, sc_y_axis_state;
/****************************************************************************/
 
// 定义两套从站的位置和类型
#define X_AXIS_POS 0, 0  // X轴位置
#define Y_AXIS_POS 0, 1  // Y轴位置
#define X5S 0x00000766, 0x00010000
#define KS100E 0x00000100, 0x00000100

// 为两套从站定义独立的PDO偏移量
static unsigned int cntlwd_x, targetpos_x, operation_mode_x, origin_mode_x;
static unsigned int statwd_x, actpos_x, modes_of_operation_display_x;
static unsigned int cur_statuswd_x, cur_mode_x;

static unsigned int cntlwd_y, targetpos_y, operation_mode_y, origin_mode_y;
static unsigned int statwd_y, actpos_y, modes_of_operation_display_y;
static unsigned int cur_statuswd_y, cur_mode_y;
 
// 扩展PDO注册表包含两套从站
ec_pdo_entry_reg_t domainServoOutput_regs[] = {
    // X轴
    {X_AXIS_POS, X5S, 0x6040, 0x00, &cntlwd_x, NULL},
    {X_AXIS_POS, X5S, 0x607a, 0x00, &targetpos_x, NULL},
    {X_AXIS_POS, X5S, 0x6060, 0x00, &operation_mode_x, NULL},
    {X_AXIS_POS, X5S, 0x6098, 0x00, &origin_mode_x, NULL},

    
    // Y轴
    {Y_AXIS_POS, KS100E, 0x6040, 0x00, &cntlwd_y, NULL},
    {Y_AXIS_POS, KS100E, 0x607a, 0x00, &targetpos_y, NULL},
    {Y_AXIS_POS, KS100E, 0x6060, 0x00, &operation_mode_y, NULL},
    {Y_AXIS_POS, KS100E, 0x6098, 0x00, &origin_mode_y, NULL},
    {}};
    
ec_pdo_entry_reg_t domainServoInput_regs[] = {
    // X轴
    {X_AXIS_POS, X5S, 0x6041, 0x00, &statwd_x, NULL},
    {X_AXIS_POS, X5S, 0x6064, 0x00, &actpos_x, NULL},
    {X_AXIS_POS, X5S, 0x6061, 0x00, &modes_of_operation_display_x, NULL},
    
    // Y轴
    {Y_AXIS_POS, KS100E, 0x6041, 0x00, &statwd_y, NULL},
    {Y_AXIS_POS, KS100E, 0x6064, 0x00, &actpos_y, NULL},
    {Y_AXIS_POS, KS100E, 0x6061, 0x00, &modes_of_operation_display_y, NULL},
    {}};
    
/****************************************************************************/
/* 更新伺服型号信息 */
ec_pdo_entry_info_t servo_pdo_entries[] = {
    {0x6040, 0x00, 16}, /* Controlword */
    {0x6060, 0x00, 8},  /* Modes of operation */
    {0x6098, 0x00, 8},
    {0x607a, 0x00, 32}, /* Target position */
    {0x60b8, 0x00, 16}, /* Touch probe function */
    {0x603f, 0x00, 16}, /* Error code */
    {0x6041, 0x00, 16}, /* Statusword */
    {0x6061, 0x00, 8},  /* Modes of operation display */
    {0x6064, 0x00, 32}, /* Position actual value */
    {0x60b9, 0x00, 16}, /* Touch probe status */
    {0x60ba, 0x00, 32}, /* Touch probe pos1 pos value */
    {0x60f4, 0x00, 32}, /* Following error actual value */
    {0x60fd, 0x00, 32}, /* Digital inputs */
};
ec_pdo_info_t servo_pdos[] = {
    {0x1600, 5, servo_pdo_entries + 0}, /* Receive PDO mapping 1 */
    {0x1a00, 8, servo_pdo_entries + 5}, /* Transmit PDO mapping 1 */
};
ec_sync_info_t servo_syncs[] = {{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
                                {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
                                {2, EC_DIR_OUTPUT, 1, servo_pdos + 0, EC_WD_ENABLE},
                                {3, EC_DIR_INPUT, 1, servo_pdos + 1, EC_WD_DISABLE},
                                {0xff}};
/****************************************************************************/
int ConfigPDO()
{
    /********************/
    printf("Configuring PDOs...\n");
    domainServoOutput = ecrt_master_create_domain(master);
    if (!domainServoOutput) {
        return -1;
    }
    domainServoInput = ecrt_master_create_domain(master);
    if (!domainServoInput) {
        return -1;
    }
    /********************/
    printf("Creating slave configurations...\n");
    // 创建X轴配置
    sc_x_axis = ecrt_master_slave_config(master, X_AXIS_POS, X5S);
    if (!sc_x_axis) {
        fprintf(stderr, "Failed to get X axis slave configuration.\n");
        return -1;
    }
    
    // 创建Y轴配置
    sc_y_axis = ecrt_master_slave_config(master, Y_AXIS_POS, KS100E);
    if (!sc_y_axis) {
        fprintf(stderr, "Failed to get Y axis slave configuration.\n");
        return -1;
    }
    /********************/
    // 配置X轴PDO
    if (ecrt_slave_config_pdos(sc_x_axis, EC_END, servo_syncs)) {
        fprintf(stderr, "Failed to configure X axis PDOs.\n");
        return -1;
    }
    
    // 配置Y轴PDO
    if (ecrt_slave_config_pdos(sc_y_axis, EC_END, servo_syncs)) {
        fprintf(stderr, "Failed to configure Y axis PDOs.\n");
        return -1;
    }
    /********************/
    if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    fprintf(stderr, "Creating SDO requests...\n");
    ecrt_slave_config_sdo8(sc_x_axis, 0x6060, 0, 6);
    ecrt_slave_config_sdo8(sc_y_axis, 0x6060, 0, 6);

    // // 设置回零方式(通过SDO)
    // // ecrt_master_sdo_download(master, 0, 0x6098, 0, &(uint8_t){35}, 1, NULL); // X轴
    // // ecrt_master_sdo_download(master, 1, 0x6098, 0, &(uint8_t){35}, 1, NULL); // Y轴
    // const uint8_t homing_method = 35;
    // ecrt_master_sdo_download(master, 0, 0x6098, 0, &homing_method, sizeof(homing_method), NULL); // X轴
    // ecrt_master_sdo_download(master, 1, 0x6098, 0, &homing_method, sizeof(homing_method), NULL); // Y轴
    return 0;
}
/*****************************************************************************
 * Realtime task
 ****************************************************************************/
void rt_check_domain_state(void)
{
    ec_domain_state_t ds = {};
    ec_domain_state_t ds1 = {};
    // domainServoInput
    ecrt_domain_state(domainServoInput, &ds);
    if (ds.working_counter != domainServoInput_state.working_counter) {
        printf("domainServoInput: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoInput_state.wc_state) {
        printf("domainServoInput: State %u.\n", ds.wc_state);
    }
    domainServoInput_state = ds;
    // domainServoOutput
    ecrt_domain_state(domainServoOutput, &ds1);
    if (ds1.working_counter != domainServoOutput_state.working_counter) {
        printf("domainServoOutput: WC %u.\n", ds1.working_counter);
    }
    if (ds1.wc_state != domainServoOutput_state.wc_state) {
        printf("domainServoOutput: State %u.\n", ds1.wc_state);
    }
    domainServoOutput_state = ds1;
}
/****************************************************************************/
void rt_check_master_state(void)
{
    ec_master_state_t ms;
    ecrt_master_state(master, &ms);
    if (ms.slaves_responding != master_state.slaves_responding) {
        printf("%u slave(s).\n", ms.slaves_responding);
    }
    if (ms.al_states != master_state.al_states) {
        printf("AL states: 0x%02X.\n", ms.al_states);
    }
    if (ms.link_up != master_state.link_up) {
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }
    master_state = ms;
}
/****************************************************************************/
void check_slave_config_states(void)
{
    ec_slave_config_state_t s;
    
    // 检查X轴状态
    ecrt_slave_config_state(sc_x_axis, &s);
    if (s.al_state != sc_x_axis_state.al_state)
        printf("X axis: State 0x%02X.\n", s.al_state);
    if (s.online != sc_x_axis_state.online)
        printf("X axis: %s.\n", s.online ? "online" : "offline");
    if (s.operational != sc_x_axis_state.operational)
        printf("X axis: %soperational.\n", s.operational ? "" : "Not ");
    sc_x_axis_state = s;
    
    // 检查Y轴状态
    ecrt_slave_config_state(sc_y_axis, &s);
    if (s.al_state != sc_y_axis_state.al_state)
        printf("Y axis: State 0x%02X.\n", s.al_state);
    if (s.online != sc_y_axis_state.online)
        printf("Y axis: %s.\n", s.online ? "online" : "offline");
    if (s.operational != sc_y_axis_state.operational)
        printf("Y axis: %soperational.\n", s.operational ? "" : "Not ");
    sc_y_axis_state = s;
}
/****************************************************************************/
void ReleaseMaster()
{
    if (master) {
        printf("End of Program, release master\n");
        ecrt_release_master(master);
        master = NULL;
    }
}
/****************************************************************************/
int ActivateMaster()
{
    int ret;
    printf("Requesting master...\n");
    if (master)
        return 0;
    master = ecrt_request_master(0);
    if (!master) {
        return -1;
    }
    ConfigPDO();
    // 配置两套从站的SYNC信号
    ecrt_slave_config_dc(sc_x_axis, 0x0300, PERIOD_NS, 0, 0, 0);
    ecrt_slave_config_dc(sc_y_axis, 0x0300, PERIOD_NS, 0, 0, 0);
    
    ecrt_master_application_time(master, system_time_ns());
    ret = ecrt_master_select_reference_clock(master, NULL);
    if (ret < 0) {
        fprintf(stderr, "Failed to select reference clock: %s\n", strerror(-ret));
        return ret;
    }
    /********************/
    printf("Activating master...\n");
    if (ecrt_master_activate(master)) {
        printf("Activating master...failed\n");
        return -1;
    }
    /********************/
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        fprintf(stderr, "Failed to get domain data pointer.\n");
        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        fprintf(stderr, "Failed to get domain data pointer.\n");
        return -1;
    }
    printf("Activating master...success\n");
    return 0;
}
/****************************************************************************/
void DriverEtherCAT()
{
    static int curpos_x = 0, curpos_y = 0;
    static int i = 0;
    // 处于刚开机(需要等待其他操作完成),返回等待下次周期
    if (gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
        return;
 
    static int cycle_counter = 0;
    cycle_counter++;
    int runtime_tick = 0;
    if (PERIOD_MS > 0) {
        runtime_tick = RUNTIME_MS / PERIOD_MS;
    } else {
        runtime_tick = 10000;
    }
    if (cycle_counter >= runtime_tick) {
        cycle_counter = 0;
        run = 0;
    }
    // receive EtherCAT frames
    ecrt_master_receive(master);
    ecrt_domain_process(domainServoOutput);
    ecrt_domain_process(domainServoInput);
    rt_check_domain_state();
    if ((cycle_counter % 500) == 0) {
        rt_check_master_state();
        check_slave_config_states();
    }
    // 状态机操作
    switch (gSysRunning.m_gWorkStatus) {
    case SYS_WORKING_SAFE_MODE:
        // 检查主站是否处于 OP 模式, 若不是,则调整为 OP 模式
        rt_check_master_state();
        check_slave_config_states();
        if ((master_state.al_states & ETHERCAT_STATUS_OP)) {
            int tmp = true;
            if (sc_x_axis_state.al_state != ETHERCAT_STATUS_OP || 
                sc_y_axis_state.al_state != ETHERCAT_STATUS_OP) {
                tmp = false;
                break;
            }
            if (tmp) {
                ecstate = 0;
                gSysRunning.m_gWorkStatus = SYS_WORKING_OP_MODE;
                printf("SYS_WORKING_OP_MODE\n");
                // X轴错误复位
                EC_WRITE_U16(domainOutput_pd + cntlwd_x, 0x80);
                // Y轴错误复位
                EC_WRITE_U16(domainOutput_pd + cntlwd_y, 0x80);
            }
        }
        break;
    case SYS_WORKING_OP_MODE:
        // 设置X轴模式
        EC_WRITE_U8(domainOutput_pd + operation_mode_x, 6);
        EC_WRITE_U8(domainOutput_pd + origin_mode_x, 35);
        cur_mode_x = EC_READ_U8(domainInput_pd + modes_of_operation_display_x);
        cur_statuswd_x = EC_READ_U16(domainInput_pd + statwd_x);
        
        // 设置Y轴模式
        EC_WRITE_U8(domainOutput_pd + operation_mode_y, 6);
        EC_WRITE_U8(domainOutput_pd + origin_mode_y, 35);
        cur_mode_y = EC_READ_U8(domainInput_pd + modes_of_operation_display_y);
        cur_statuswd_y = EC_READ_U16(domainInput_pd + statwd_y);
        
        // X轴状态处理
        if ((cur_statuswd_x & 0x004f) == 0x0040) {
            EC_WRITE_U16(domainOutput_pd + cntlwd_x, 0x06);
        } else if ((cur_statuswd_x & 0x006f) == 0x0021) {
            EC_WRITE_U16(domainOutput_pd + cntlwd_x, 0x07);
        } else if ((cur_statuswd_x & 0x006f) == 0x023) {
            EC_WRITE_U16(domainOutput_pd + cntlwd_x, 0x0F);
            curpos_x = EC_READ_S32(domainInput_pd + actpos_x);
        } else if ((cur_statuswd_x & 0x006f) == 0x0027) {
            EC_WRITE_U16(domainOutput_pd + cntlwd_x, 0x001f);
            curpos_x = EC_READ_S32(domainInput_pd + actpos_x);
            // EC_WRITE_S32(domainOutput_pd + targetpos_x, curpos_x);
            printf("X axis current position = %d\n", curpos_x);
        }
        
        // Y轴状态处理
        if ((cur_statuswd_y & 0x004f) == 0x0040) {
            EC_WRITE_U16(domainOutput_pd + cntlwd_y, 0x06);
        } else if ((cur_statuswd_y & 0x006f) == 0x0021) {
            EC_WRITE_U16(domainOutput_pd + cntlwd_y, 0x07);
        } else if ((cur_statuswd_y & 0x006f) == 0x023) {
            EC_WRITE_U16(domainOutput_pd + cntlwd_y, 0x0F);
            curpos_y = EC_READ_S32(domainInput_pd + actpos_y);
        } else if ((cur_statuswd_y & 0x006f) == 0x0027) {
            EC_WRITE_U16(domainOutput_pd + cntlwd_y, 0x001f);
            curpos_y = EC_READ_S32(domainInput_pd + actpos_y);
            // EC_WRITE_S32(domainOutput_pd + targetpos_y, curpos_y);
            printf("Y axis current position = %d\n", curpos_y);
        }

        // 检查两轴是否都准备好
        if (((cur_statuswd_x & STATUS_SERVO_ENABLE_BIT) != 0) && 
            ((cur_statuswd_y & STATUS_SERVO_ENABLE_BIT) != 0)) {
            ecstate = 0;
            gSysRunning.m_gWorkStatus = SYS_WORKING_IDLE_STATUS;
            printf("SYS_WORKING_IDLE_STATUS\n");
        }
        break;
    default:
        // 读取两轴当前位置
        cur_statuswd_x = EC_READ_U16(domainInput_pd + statwd_x);
        cur_statuswd_y = EC_READ_U16(domainInput_pd + statwd_y);
        // if ((cycle_counter % 250) == 0) {
            printf("X_curpos = %d \t", EC_READ_S32(domainInput_pd + actpos_x));
            printf("Y_curpos = %d \n", EC_READ_S32(domainInput_pd + actpos_y));
        // }
        
        // curpos_x += 131;
        // curpos_y += 131;
        
        
        // EC_WRITE_S32(domainOutput_pd + targetpos_x, curpos_x);
        // EC_WRITE_S32(domainOutput_pd + targetpos_y, curpos_y);
        break;
    }
    // write application time to master
    ecrt_master_application_time(master, system_time_ns());
    ecrt_master_sync_reference_clock(master);
    ecrt_master_sync_slave_clocks(master);
    // send process data
    ecrt_domain_queue(domainServoOutput);
    ecrt_domain_queue(domainServoInput);
    ecrt_master_send(master);
}
/****************************************************************************/
void *InterpolationThread(void *arg)
{
    struct timespec wait, previous;
    clock_gettime(CLOCK_REALTIME, &previous);
    wait = previous;
    while (run) {
        wait.tv_nsec += PERIOD_NS; // 1ms
        if (wait.tv_nsec >= NSEC_PER_SEC) {
            wait.tv_sec++;
            wait.tv_nsec -= NSEC_PER_SEC;
        }
        // Delay the calling task (absolute). Delay the execution of the calling task until a given
        // date is reached.
        clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &wait, NULL);
        DriverEtherCAT();
    }
    return NULL;
}
/****************************************************************************
 * Signal handler
 ***************************************************************************/
void signal_handler(int sig)
{
    run = 0;
}
/****************************************************************************
 * Main function
 ***************************************************************************/
int main(int argc, char *argv[])
{
    int ret;
    signal(SIGTERM, signal_handler);
    signal(SIGINT, signal_handler);
    /*
        内存锁定
        MCL_CURRENT锁定当前内存，MCL_FUTURE锁定后续分配的内存
    */
    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
        perror("mlockall failed");
        return -1;
    }
    
    /*
        主站初始化
    */
    gSysRunning.m_gWorkStatus = SYS_WORKING_POWER_ON;
    if (gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON) {
        ActivateMaster();
        ecstate = 0;
        gSysRunning.m_gWorkStatus = SYS_WORKING_SAFE_MODE;
        printf("SYS_WORKING_SAFE_MODE\n");
    }
    /*
        线程设置，最高优先级，可抢占
    */
    pthread_attr_t attr;
    struct sched_param param;
    pthread_attr_init(&attr);
    pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
    param.sched_priority = 99;
    pthread_attr_setschedparam(&attr, &param);
    pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    ret = pthread_create(&InterpolationTask, &attr, InterpolationThread, NULL);
    if (ret) {
        fprintf(stderr, "Failed to create task: %s\n", strerror(ret));
        return -1;
    }
    
    printf("Starting InterpolationTask...\n");
    while (run) {
        usleep(50000);
    }
    printf("Deleting realtime InterpolationTask task...\n");
    pthread_join(InterpolationTask, NULL);
    ReleaseMaster();
    return 0;
}